\section*{\large Navigation Node}
\begin{normalsize}


The navigation node reads the map parameters from the configuration file nxt\_lab2.yaml loaded from the launcher file nxt\_lab2.launch, see sections Yaml description and Launch description, and implements the wave-front propagation algorithm, which uses dynamic programming over an occupancy grid map to find the lowest-cost path from the robot's pose to the goal. Its main steps are:
\begin{itemize}
	\item fetching the starting map from the configuration file: 0 designates a cell of free space, -2 a cell occupied by an obstacle, -1 the start and 1 the goal;
	\item filling the map starting from the goal: cells are labelled with increasing numbers in breadth-first search order until start is reached;
	\item finding the shortest path, that is a decreasing path from the start to the goal;
\end{itemize}

% TODO: IMMAGINE

Two modes can be chosen from the configuration file, static or dynamic navigation. They are described by the flowcharts in figures \ref{fig:static} and \ref{fig:dynamic}. 
Concerning adaptive mapping, the robot checks for added and removed obstacles in its von Neumann neighbourhood and react properly. If trapped by obstacles, it does not surrender, erases the map, that is its knowledge of the environment, and retry to find the shortest path from its current position to the goal.

\begin{figure}[!htb]
\centering
\begin{minipage}{.35\linewidth}
  \includegraphics[width=\linewidth]{./images/flowchart_static.png}
  \caption{static map \label{fig:static}}
\end{minipage}
\hspace{.05\linewidth}
\begin{minipage}{.35\linewidth}
  \includegraphics[width=\linewidth]{./images/flowchart_dynamic.png}
  \caption{dynamic map \label{fig:dynamic}}
\end{minipage}
\end{figure}

Being quite time consuming to test the program on the actual robot, this node can emulate the robot services get\_neighbors and move\_to\_cell, see functions emulate\_movement and emulate\_head. In the following, the description of every function is given:

\begin{itemize}

\item \textbf{Printing functions}:

\begin{itemize}



\item \textbf{send\_updated\_map}:
\begin{itemize} 
    \item Description: This function provides the service for sending the updated map, it takes in the request and response type defined in the srv file and returns a boolean.
 \end{itemize}



\item \textbf{printVec}:
\begin{itemize}
\item Inputs: 

     \begin{itemize}
	\item std::vector$<$int$>$ \& v: vector to be printed
     \end{itemize}

    \item Description: Print vector of int

\end{itemize}



\item \textbf{printMap}:
\begin{itemize}

     \item Inputs: 

     \begin{itemize}
      	\item std::vector$<$int$>$ \& map: vector representing matrix
     \end{itemize}

     \item Description: Print matrix represented as vector

\end{itemize}



\end{itemize}
\item \textbf{Navigation functions}:
\begin{itemize}



\item \textbf{find\_start}:
\begin{itemize}
    
    \item Inputs: 

     \begin{itemize}
      	\item std::vector$<$int$>$ \& v: vector representing map
      	\item int \& start\_index: index of found start
     \end{itemize}

	\item Description: Find start index (from 0 to rows * cols - 1)
    return true if found, false otherwise

\end{itemize}



\item \textbf{find\_goal}:
\begin{itemize}

\item Inputs: 

     \begin{itemize}
     	\item std::vector$<$int$>$ \& v: vector representing map
      	\item int \& goal\_index: index of found goal
     \end{itemize}
  
    \item Description: Find goal index (from 0 to rows * cols - 1)
    return true if found, false otherwise
 \end{itemize}



\item \textbf{get\_row\_index}:
\begin{itemize}

\item Inputs: 

     \begin{itemize}
     	 \item int pos: vector position pos
     \end{itemize}
  
    \item Description: Return row index (from 0 to rows - 1) of vector position pos
 \end{itemize}



\item \textbf{get\_neighbors}:
\begin{itemize}
   
    \item Inputs: 

     \begin{itemize}
      	\item int pos: index of cell
      	\item std::vector$<$int$>$ \& map: vector representing map
      std::list$<$ std::pair $<$int,int$>$ $>$ \& neighbors: indexes of
      	\item std::list$<$ std::pair $<$int,int$>$ $>$ \& neighbors: indexes of neighbors of cell
      	\item bool only\_empty: if true, only neighbors with empty value and start value are returned, option exploited by fillMap
     \end{itemize}

	 \item Description: Find indexes of neighbors of cell with index pos (first element of pairs)
    storing its parent (second element of each pair), useful when it is called by fillMap
    and filtering obstacles

\end{itemize}



\item \textbf{clear\_map}:
\begin{itemize}
   
    \item Inputs: 

     \begin{itemize}
      	\item std::vector$<$int$>$ \& map: map to be cleared
     \end{itemize}

	 \item Description: Clear map by
   filling it with empty values except start, goal and obstacles

\end{itemize}



\item \textbf{fillMap}:
\begin{itemize}
   
    \item Inputs: 

     \begin{itemize}
      	\item  std::vector$<$int$>$ \& map: map to be filled
     \end{itemize}

   \item Description: Wavefront algorithm fill map with numbers return steps\_to\_goal (min is 1) if path is found, 0 otherwise

\end{itemize}




\item \textbf{find\_shortest\_path}:
\begin{itemize}
    
    \item Inputs: 

     \begin{itemize}
      	\item  std::vector$<$int$>$ \& path: path represented as sequence of int cardinal directions (north 0 east 1 south 2 west 3)
      	\item std::vector$<$int$>$ \& pos\_path: pos\_path represented as sequence of indexes of vector representing map
      	\item std::vector$<$int$>$ \& map: map already filled with numbers by fillMap
      	\item  int steps\_to\_goal: number of steps to goal
     \end{itemize}

	\item Description: Find shortest path

\end{itemize}



\item \textbf{try\_movement}:
\begin{itemize}
    
       \item Inputs: 

     \begin{itemize}
      	\item ros::ServiceClient \& client: robot service move\_to\_cell
      	\item nxt\_lab2::MoveToCell \& cell\_srv: move\_to\_cell message request
      	\item int cardinal\_direction: movement cardinal direction
     \end{itemize}

	\item Description: Try movement

\end{itemize}



\end{itemize}
\item \textbf{Dynamic map functions}:
\begin{itemize}



\item \textbf{update\_map}:
\begin{itemize}
    
      \item Inputs: 

     \begin{itemize}
      	\item std::vector$<$int$>$ \& map: map to be updated
      	\item std::vector$<$int$>$ \& neighbors: vector of binary values (0 if no obstacle detected, 1 if obstacle detected) of length 4, one value for each cell in clockwise order
         \item int nxt\_pos: robot position
     \end{itemize}

	\item Description: Update map with added or removed obstacles

\end{itemize}



\item \textbf{erase\_map}:
\begin{itemize}
    
      \item Inputs: 

     \begin{itemize}
      	\item std::vector$<$int$>$ \& map: map to be erased
     \end{itemize}

	\item Description: Erase map, fill map with empty values except start and goal

\end{itemize}


\end{itemize}
\item \textbf{Robot emulation}:
\begin{itemize}



\item \textbf{emulate\_movement}:
\begin{itemize}
	\item Description: Emulate robot service moveToCellSrv and navigation function try\_movement
\end{itemize}



\item \textbf{emulate\_head}:
\begin{itemize}
	\item Description: Emulate robot service getNeighborsSrv
\end{itemize}



\end{itemize}



\end{itemize}

\end{normalsize}